﻿#include <occmesh/api/coons.h>

TopoDS_Face Coons::Covering(Wire *wire)
{
    if (!PrepareBoundaryCurves(wire))
        return TopoDS_Face{};
    PrepareDerivateFields(wire);

    // 累计控制点
    dstPoly.Resize(1, boundaryCurves[0]->NbPoles(), 1, boundaryCurves[1]->NbPoles(), false);
    dstPoly.Init(gp_Pnt(0, 0, 0));

    // 构建 S1，S2 并累计
    for (int i = 1; i <= boundaryCurves[0]->NbPoles(); i++)
        AccumulatePoint(boundaryCurves[1], boundaryCurves[0]->Pole(i),
                        boundaryCurves[2]->Pole(boundaryCurves[2]->NbPoles() + 1 - i), derivateFields[0]->Pole(i).XYZ(),
                        derivateFields[2]->Pole(derivateFields[2]->NbPoles() + 1 - i).XYZ(), i, 'r');
    for (int i = 1; i <= boundaryCurves[1]->NbPoles(); i++)
        AccumulatePoint(boundaryCurves[0], boundaryCurves[3]->Pole(boundaryCurves[3]->NbPoles() + 1 - i),
                        boundaryCurves[1]->Pole(i), derivateFields[3]->Pole(derivateFields[3]->NbPoles() + 1 - i).XYZ(),
                        derivateFields[1]->Pole(i).XYZ(), i, 'c');

    auto T = BuildBilinearSurface();
    for (int i = 1; i <= boundaryCurves[0]->NbPoles(); i++)
        for (int j = 1; j <= boundaryCurves[1]->NbPoles(); j++)
            dstPoly.SetValue(i, j, dstPoly.Value(i, j).XYZ() - T->Pole(i, j).XYZ());

    // 构造最终曲面
    Handle(Geom_BSplineSurface) sur(new Geom_BSplineSurface(
        dstPoly, boundaryCurves[0]->Knots(), boundaryCurves[1]->Knots(), boundaryCurves[0]->Multiplicities(),
        boundaryCurves[1]->Multiplicities(), boundaryCurves[0]->Degree(), boundaryCurves[1]->Degree()));

    return BRepBuilderAPI_MakeFace(sur, 1e-16);
}

bool Coons::PrepareBoundaryCurves(Wire *wire)
{
    auto boundaryEdges = wire->Edges();
    if (boundaryEdges.size() != 4)
        return false;

    // 边界曲线至少是 3 次
    double first, second;
    for (int i = 0; i < boundaryEdges.size(); i++)
    {
        boundaryCurves[i] = Handle(Geom_BSplineCurve)::DownCast(BRep_Tool::Curve(boundaryEdges[i], first, second));
        if (boundaryCurves[i]->Degree() < 3)
            boundaryCurves[i]->IncreaseDegree(3);
    }
    return true;
}

void Coons::PrepareDerivateFields(Wire *wire)
{
    // 计算沿着边界的切向场（在 Rebuild 阶段已经去除法向分量）
    gp_Vec startTangent[4], endTangent[4];
    int signs[] = {1, -1, -1, 1};

    // 获得切向场（暂时用边界切向插值）
    // 注意所有切向以 u,v 增量方向为正
    auto halfEdge = wire->halfEdge;
    for (int i = 0; i < 4; i++)
    {
        startTangent[i] = halfEdge->StartPerpTangent() * signs[i];
        endTangent[i] = halfEdge->EndPerpTangent() * signs[i];
        halfEdge = halfEdge->next;
    }

    for (int i = 0; i < 4; i++)
    {
        TColStd_Array1OfInteger multiplier(1, 2);
        TColStd_Array1OfReal knots(1, 2);
        TColgp_Array1OfPnt poles(1, 2);

        // 切向线性插值
        multiplier.SetValue(1, 2);
        multiplier.SetValue(2, 2);
        knots.SetValue(1, boundaryCurves[i]->FirstParameter());
        knots.SetValue(2, boundaryCurves[i]->LastParameter());
        poles.SetValue(1, startTangent[i].XYZ());
        poles.SetValue(2, endTangent[i].XYZ());

        derivateFields[i] = new Geom_BSplineCurve(poles, knots, multiplier, 1);
        derivateFields[i]->IncreaseDegree(boundaryCurves[i]->Degree());

        for (int j = 2; j < boundaryCurves[i]->NbKnots(); j++)
            derivateFields[i]->InsertKnot(boundaryCurves[i]->Knot(j), boundaryCurves[i]->Multiplicity(j));
    }
}

void Coons::InterpInteriorPoint(const gp_Pnt &p1, const gp_Pnt &p2, const gp_Vec &v1, const gp_Vec &v2, gp_Pnt *poles)
{
    poles[0] = p1;
    poles[1] = p1.XYZ() + v1.XYZ() / 3;
    poles[2] = p2.XYZ() - v2.XYZ() / 3;
    poles[3] = p2;
}

void Coons::AccumulatePoint(Handle(Geom_BSplineCurve) cur, const gp_Pnt &p1, const gp_Pnt &p2, const gp_Vec &v1,
                            const gp_Vec &v2, int k, char rc)
{
    int deg = cur->Degree();

    TColStd_Array1OfReal knots(1, 2);
    knots.SetValue(1, cur->FirstParameter());
    knots.SetValue(2, cur->LastParameter());

    TColStd_Array1OfInteger multiplier(1, 2);
    multiplier.SetValue(1, 4);
    multiplier.SetValue(2, 4);

    // 插值控制点
    gp_Pnt interpPoints[4];
    InterpInteriorPoint(p1, p2, v1, v2, interpPoints);
    TColgp_Array1OfPnt poles(1, 4);
    for (int i = 1; i <= poles.Size(); i++)
        poles.SetValue(i, interpPoints[i - 1]);

    // 得到 Bezier 曲线
    Handle(Geom_BSplineCurve) bez(new Geom_BSplineCurve(poles, knots, multiplier, 3));
    bez->IncreaseDegree(deg);

    // 节点插入
    for (int i = 2; i < cur->NbKnots(); i++)
        bez->InsertKnot(cur->Knot(i), cur->Multiplicity(i));

    // 填充控制点
    const auto &bezPoles = bez->Poles();
    if (rc == 'r')
        for (int i = 1; i <= bez->NbPoles(); i++)
            dstPoly.SetValue(k, i, dstPoly.Value(k, i).XYZ() + bezPoles.Value(i).XYZ());
    else
        for (int i = 1; i <= bez->NbPoles(); i++)
            dstPoly.SetValue(i, k, dstPoly.Value(i, k).XYZ() + bezPoles.Value(i).XYZ());
}

Handle(Geom_BSplineSurface) Coons::BuildBilinearSurface()
{
    // 计算扭矢量
    gp_Pnt P;
    gp_Vec twists0[4], twists1[4];
    derivateFields[0]->D1(boundaryCurves[0]->FirstParameter(), P, twists0[0]);
    derivateFields[1]->D1(boundaryCurves[1]->FirstParameter(), P, twists0[1]);
    derivateFields[2]->D1(boundaryCurves[2]->FirstParameter(), P, twists0[2]);
    derivateFields[3]->D1(boundaryCurves[3]->FirstParameter(), P, twists0[3]);

    derivateFields[0]->D1(boundaryCurves[0]->LastParameter(), P, twists1[0]);
    derivateFields[1]->D1(boundaryCurves[1]->LastParameter(), P, twists1[1]);
    derivateFields[2]->D1(boundaryCurves[2]->LastParameter(), P, twists1[2]);
    derivateFields[3]->D1(boundaryCurves[3]->LastParameter(), P, twists1[3]);

    // 水平插值得到内部控制点
    gp_Pnt interpPoints[16];
    for (int i = 0; i < 4; i++)
    {
        int j = (i + 3) % 4;
        int k = (i + 1) % 4;

        // 反转标记
        int sign = i > 1 ? -1 : 1;
        InterpInteriorPoint(boundaryCurves[i]->StartPoint(), boundaryCurves[i]->EndPoint(),
                            sign * derivateFields[j]->EndPoint().XYZ(), sign * derivateFields[k]->StartPoint().XYZ(),
                            &interpPoints[i * 4]);
    }

    // 重整控制点
    TColgp_Array2OfPnt bilinearPoles(1, 4, 1, 4);
    for (int i = 0; i < 4; i++)
    {
        bilinearPoles.SetValue(i + 1, 1, interpPoints[i]);      // 上
        bilinearPoles.SetValue(4 - i, 4, interpPoints[8 + i]);  // 下
        bilinearPoles.SetValue(4, i + 1, interpPoints[4 + i]);  // 左
        bilinearPoles.SetValue(1, 4 - i, interpPoints[12 + i]); // 右
    }

    // 填充内部控制点
    bilinearPoles.SetValue(2, 2,
                           bilinearPoles.Value(1, 2).XYZ() + bilinearPoles.Value(2, 1).XYZ() -
                               bilinearPoles.Value(1, 1).XYZ() + (twists0[0].XYZ() + twists1[3].XYZ()) / 18);
    bilinearPoles.SetValue(3, 2,
                           bilinearPoles.Value(4, 2).XYZ() + bilinearPoles.Value(3, 1).XYZ() -
                               bilinearPoles.Value(4, 1).XYZ() + (twists0[1].XYZ() + twists1[0].XYZ()) / 18);
    bilinearPoles.SetValue(3, 3,
                           bilinearPoles.Value(4, 3).XYZ() + bilinearPoles.Value(3, 4).XYZ() -
                               bilinearPoles.Value(4, 4).XYZ() + (twists0[2].XYZ() + twists1[1].XYZ()) / 18);
    bilinearPoles.SetValue(2, 3,
                           bilinearPoles.Value(1, 3).XYZ() + bilinearPoles.Value(2, 4).XYZ() -
                               bilinearPoles.Value(1, 4).XYZ() + (twists0[3].XYZ() + twists1[2].XYZ()) / 18);

    // 节点
    TColStd_Array1OfReal uknots(1, 2);
    TColStd_Array1OfReal vknots(1, 2);
    TColStd_Array1OfInteger umults(1, 2);
    TColStd_Array1OfInteger vmults(1, 2);

    uknots.SetValue(1, boundaryCurves[0]->FirstParameter());
    uknots.SetValue(2, boundaryCurves[0]->LastParameter());
    vknots.SetValue(1, boundaryCurves[1]->FirstParameter());
    vknots.SetValue(2, boundaryCurves[1]->LastParameter());

    umults.SetValue(1, 4);
    umults.SetValue(2, 4);
    vmults.SetValue(1, 4);
    vmults.SetValue(2, 4);

    Handle(Geom_BSplineSurface) T(new Geom_BSplineSurface(bilinearPoles, uknots, vknots, umults, vmults, 3, 3));
    T->IncreaseDegree(boundaryCurves[0]->Degree(), boundaryCurves[1]->Degree());

    // 节点插入
    for (int i = 2; i < boundaryCurves[0]->NbKnots(); i++)
        T->InsertUKnot(boundaryCurves[0]->Knot(i), boundaryCurves[0]->Multiplicity(i), 1e-6);
    for (int i = 2; i < boundaryCurves[1]->NbKnots(); i++)
        T->InsertVKnot(boundaryCurves[1]->Knot(i), boundaryCurves[1]->Multiplicity(i), 1e-6);

    return T;
}